#include "calc.h"
#include <stdio.h>

using namespace cv;
using namespace std;
namespace enc = sensor_msgs::image_encodings;
using namespace cv;




class CalcCenterNodeImpl : public CalcCenterNode
{
  ros::NodeHandle nh_;
  ros::Subscriber caminfo_sub_;
  ros::Subscriber pointcloud2_sub_;
  ros::Publisher PoseStamped_pub_;


public:
  CalcCenterNodeImpl()
  {
    pointcloud2_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("point_cloud",1,&CalcCenterNodeImpl::pointcloudCb,this);
    PoseStamped_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("calc_center",1);

  }

  ~CalcCenterNodeImpl(){

  }


  virtual void pointcloudCb(const sensor_msgs::PointCloud2ConstPtr &msg)
  {
    geometry_msgs::PoseStamped out_msg;
    //pcl::PointCloud<pcl::PointXYZRGBNormal> points;
    pcl::PointCloud<pcl::PointXYZ> points;
    //pcl::PointCloud<pcl::PointXYZ> points;
    //pcl::PointCloud<pcl::PointXYZRGBNormal> normal_points;
    pcl::PointCloud<pcl::PointXYZ> normal_points;
    //pcl::PointCloud<pcl::PointXYZ> normal_points;
    pcl::fromROSMsg(*msg, points);
    out_msg.header = points.header;
    out_msg.pose.orientation.x = 1.0;
    out_msg.pose.orientation.y = 0.0;
    out_msg.pose.orientation.z = 0.0;
    out_msg.pose.orientation.w = 0.0;


    //pcl::fromROSMsg(*msg, normal_points);
    pcl::PointXYZRGB p;
    p.x = 0;
    p.y = 0;
    p.z = 0;
    
    for(size_t i = 0; i < points.points.size(); i++)
      {

	//pcl::PointXYZ p;
    	//pcl::PointXYZRGBNormal p;
    	p.x += points.points[i].x;
        p.y += points.points[i].y;
        p.z += points.points[i].z;
	// p.normal[0] = points.points[i].normal[0];
        // p.normal[1] = points.points[i].normal[1];
        // p.normal[2] = points.points[i].normal[2];
        //p.rgb = points.points[i].rgb;
	//ROS_ERROR("(%f %f %f) ", p.x, p.y, p.z);
	// ROS_ERROR("(%f %f %f) ", points.points[i].x
	// 	  , points.points[i].y
	// 	  , points.points[i].z);

      }
    
    


    out_msg.pose.position.x = (double)(p.x / points.points.size());
    out_msg.pose.position.y = (double)(p.y / points.points.size());
    out_msg.pose.position.z = (double)(p.z / points.points.size());

    
    PoseStamped_pub_.publish(out_msg);
  }
  
};

CalcCenterNodePtr CreateCalcCenterNode()
{
    return CalcCenterNodePtr(new CalcCenterNodeImpl());
}
